﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Track {

    private List<Vector3> gTrackList ;
    //private List<int> gTrackIndexList;

    public Track()
    {
        //gTrackIndexList = new List<int>();
        gTrackList = new List<Vector3>();
        InitTrack();
    }

    /// <summary>
    /// 寻找最短路径
    /// </summary>
    public void Search(int iStartPosition,int iEndPosition ,out List<Vector3> oCarTrackList,out List<int> oTrackIndexList)
    {
        oCarTrackList = new List<Vector3>();
        oTrackIndexList = new List<int>();
        //1、正向寻找，计算距离
        double pDistanceTotal1 = TwoTrackPositionDistance(iStartPosition, iEndPosition);
        //2、反向寻找，计算距离
        double pDistanceTotal2 = TwoTrackPositionDistance(iEndPosition, iStartPosition);
       
        //两条路线进行判断
        if(pDistanceTotal1 < pDistanceTotal2)//正向路线距离更加短
        {
            oTrackIndexList = SearchTrack(iStartPosition, iEndPosition);
        }
        else//反向路线距离更加短
        {
            oTrackIndexList = SearchTrackByReverse(iStartPosition, iEndPosition);
        }
        //小车运行路线
        for(int i=0;i< oTrackIndexList.Count;i++)
        {
            oCarTrackList.Add(gTrackList[oTrackIndexList[i]]);
        }
    }

    /// <summary>
    /// 添加最短路径节点
    /// </summary>
    /// <param name="iStartPosition"></param>
    /// <param name="iEndPosition"></param>
    /// <returns></returns>
    private List<int> SearchTrack(int iStartPosition, int iEndPosition)
    {
        List<int> gTrackIndexList = new List<int>();
        if (iStartPosition > iEndPosition)
        {
            for (int i = iStartPosition+1; i <gTrackList.Count; i++)
            {
                gTrackIndexList.Add(i);
            }
            for (int i = 0; i <= iEndPosition; i++)
            {
                gTrackIndexList.Add(i);
            }

        }
        else if(iStartPosition < iEndPosition)
        {
            for (int i = iStartPosition + 1; i <=iEndPosition; i++)
            {
                gTrackIndexList.Add(i);
            }
        }

        return gTrackIndexList;
    }

    private List<int> SearchTrackByReverse(int iStartPosition, int iEndPosition)
    {
        List<int> gTrackIndexList = new List<int>();
        if (iStartPosition > iEndPosition)
        {

            for (int i = iStartPosition - 1; i >= iEndPosition; i--)
            {
                gTrackIndexList.Add(i);
            }
        }
        else if(iStartPosition < iEndPosition)
        {

            for (int i = iStartPosition - 1; i >= 0; i--)
            {
                gTrackIndexList.Add(i);
            }

            for (int i = gTrackList.Count - 1; i >= iEndPosition; i--)
            {
                gTrackIndexList.Add(i);
            }
        }

        return gTrackIndexList;
    }

    /// <summary>
    /// 依据路径轨迹，计算两AGV小车路径
    /// </summary>
    /// <param name="iStartPosition"></param>
    /// <param name="iEndPosition"></param>
    /// <returns></returns>
    private float TwoTrackPositionDistance(int iStartPosition, int iEndPosition)
    {
        float pDistanceTotal = 0.0f;
        if (iStartPosition > iEndPosition)
        {
            for (int i = iStartPosition; i < gTrackList.Count-1; i++)
            {
                pDistanceTotal = pDistanceTotal + Vector3.Distance(gTrackList[i], gTrackList[i + 1]);
            }
            for (int i = 0; i < iEndPosition; i++)
            {
                pDistanceTotal = pDistanceTotal + Vector3.Distance(gTrackList[iStartPosition], gTrackList[iStartPosition + 1]);
            }
        }
        else
        {
            for (int i = iStartPosition; i < iEndPosition; i++)
            {
                pDistanceTotal = pDistanceTotal + Vector3.Distance(gTrackList[i], gTrackList[i + 1]);
            }
        }

        return pDistanceTotal;
    }

    /// <summary>
    /// 初始化路径
    /// </summary>
    private void InitTrack()
    {
        Vector3 pAGVPosition0 = new Vector3(-300f, 15.4f, -150f);
        Vector3 pAGVPosition1 = new Vector3(-150f, 15.4f, -150f);
        Vector3 pAGVPosition2 = new Vector3(150f, 15.4f, -150f);
        Vector3 pAGVPosition3 = new Vector3(300f, 15.4f, -150f);
        Vector3 pAGVPosition4 = new Vector3(300f, 15.4f, 150f);
        Vector3 pAGVPosition5 = new Vector3(150f, 15.4f, 150f);
        Vector3 pAGVPosition6 = new Vector3(-150f, 15.4f, 150f);
        Vector3 pAGVPosition7 = new Vector3(-300f, 15.4f, 150f);

        gTrackList.Add(pAGVPosition0);
        gTrackList.Add(pAGVPosition1);
        gTrackList.Add(pAGVPosition2);
        gTrackList.Add(pAGVPosition3);
        gTrackList.Add(pAGVPosition4);
        gTrackList.Add(pAGVPosition5);
        gTrackList.Add(pAGVPosition6);
        gTrackList.Add(pAGVPosition7);
       // gTrackList.Add(pAGVPosition0);
    }

    ///// <summary>
    ///// 
    ///// </summary>
    ///// <returns></returns>
    //public bool IsEndTrack()
    //{
    //    if (gTrackList.Count == 0)
    //    {
    //        return true;
    //    }
    //    return false;
    //}

    /// <summary>
    /// 判断是否有下一个点
    /// </summary>
    /// <returns></returns>
    //public void NextTargetPosition( out bool oIsEnd,out Vector3 oNextPosition)
    //{
    //    if(gTrackIndexList.Count ==0)
    //    {
    //        oIsEnd=true ;
    //        oNextPosition = new Vector3();
    //    }
    //    else
    //    {
    //        oIsEnd = false;
    //        oNextPosition = gTrackList[gTrackIndexList[0]];
    //        gTrackIndexList.RemoveAt(0);
    //    }
    //}

}
